#include "odespacewrap.h"
#include "physical.h"
#include <ode/collision_space.h>
#include <ode/collision.h>
#include <ode/objects.h>
#include "transformable.h"

namespace CoreBase
{
	ODESpaceWrap::ODESpaceWrap(ODEWorldWrap* worldWrapper): _spaceID(0)
		, _userNearCallback(NULL) , _contactJointGroupID(0), _worldWrapper(worldWrapper)
	{
		_spaceID = dHashSpaceCreate(0);
		dSpaceSetCleanup(_spaceID, 0);
		_contactJointGroupID = dJointGroupCreate(0);
	}

	ODESpaceWrap::~ODESpaceWrap()
	{
		dSpaceDestroy(_spaceID);
		dJointGroupDestroy(_contactJointGroupID);
	}

	void ODESpaceWrap::registerCollidable(Transformable* collidable)
	{
		if (collidable == NULL) 
		{
			return;
		}
		dSpaceAdd(_spaceID, collidable->getGeomID());
		dGeomSetData(collidable->getGeomID(), collidable);
	}

	void ODESpaceWrap::unRegisterCollidable(Transformable* collidable)
	{
		if (collidable == NULL) { return; }
		dSpaceRemove(_spaceID, collidable->getGeomID());
	}

	void ODESpaceWrap::collide()
	{
		if (_userNearCallback)
		{
			dSpaceCollide(_spaceID, _userNearCallbackData, _userNearCallback);
		}
		else
		{
			dSpaceCollide(_spaceID, this, defaultNearCallback);
		}
	}

	void ODESpaceWrap::setUserCollisionCallback(dNearCallback* func, void* data)
	{
		_userNearCallback = func;
		_userNearCallbackData = data;
	}

	void ODESpaceWrap::defaultNearCallback(void* data, dGeomID o1, dGeomID o2)
	{
		if (data == 0 || o1 == 0 || o2 == 0)
		{
			return;
		}
		ODESpaceWrap* spaceWrap = static_cast<ODESpaceWrap*>(data);
		Transformable* c1 = static_cast<Transformable*>(dGeomGetData(o1));
		Transformable* c2 = static_cast<Transformable*>(dGeomGetData(o2));
		dContactGeom contactGeoms[8];
		int numContacts = dCollide(o1, o2, 8, contactGeoms, sizeof(dContactGeom));
		if (numContacts > 0 && c1 != 0 && c2 != 0)
		{
			CollisionData cd;
			cd._bodies[0] = c1;
			cd._bodies[1] = c2;
			cd._location.set(
				contactGeoms[0].pos[0], contactGeoms[0].pos[1], contactGeoms[0].pos[2]
			);
			cd._normal.set(
				contactGeoms[0].normal[0], contactGeoms[0].normal[1], contactGeoms[0].normal[2]
			);
			cd._depth = contactGeoms[0].depth;
			PramBase* pram = new Parm1<CollisionData>(cd);
			if (spaceWrap->_callbackFunc.valid())
			{
				spaceWrap->_callbackFunc->callFunc(pram);
			}
			if (c1 != 0 || c2 != 0)
			{
				dContact contact;
				for (int i = 0; i < numContacts; ++i)
				{
					contact.surface.mode       = dContactBounce;
					contact.surface.mu         = (dReal)1000.0;
					contact.surface.bounce     = (dReal)0.75;
					contact.surface.bounce_vel = (dReal)0.001;
					contact.geom = contactGeoms[i];
					// Make sure to call these both, because in the case of
					// Trigger, meaningful stuff happens even if the return
					// is false.
					bool contactResult1 = c1->filterContact(&contact, c2);
					bool contactResult2 = c2->filterContact(&contact, c1);
					if (contactResult1 && contactResult2)
					{
						// All this also should be in a virtual function.
						Physical* p1 = dynamic_cast<Physical*>(c1);
						Physical* p2 = dynamic_cast<Physical*>(c2);
						if (p1 != 0 || p2 != 0)
						{
							dJointID joint = dJointCreateContact(spaceWrap->_worldWrapper->getWorldID(),
								spaceWrap->_contactJointGroupID,
								&contact);
							dJointAttach(joint,
								p1 != 0 && p1->dynamicsEnabled() ? p1->getBodyID() : 0,
								p2 != 0 && p2->dynamicsEnabled() ? p2->getBodyID() : 0);
						}
					}
				}
			}
		}
	}

	dNearCallback* ODESpaceWrap::getUserCollisionCallback() const
	{
		return _userNearCallback;
	}

	void* ODESpaceWrap::getUserCollisionData()
	{
		return _userNearCallbackData;
	}

	const void* ODESpaceWrap::getUserCollisionData() const
	{
		return _userNearCallbackData;
	}

	void ODESpaceWrap::postCollide()
	{
		dJointGroupEmpty(_contactJointGroupID);
	}

	dJointGroupID ODESpaceWrap::getContactJoinGroupID() const
	{
		return _contactJointGroupID;
	}

	dSpaceID ODESpaceWrap::getSpaceID() const
	{
		return _spaceID;
	}
}